
# %% [markdown]
#
import numpy as np
import matplotlib.pyplot as plt
import control as ct
import sympy as sp

if __name__ == "__main__":
    pass
    # %% [markdown]
    #
    Ts_ = 1
    d_depth_ = 1
    fc_ = 1
    zeta_0N_ = 1
    zeta_0D_ = 1

    Ts, fc, d_depth = sp.symbols('Ts f_c d_depth')
    # d_depth, f_width, f0 = d_depth_, f_width_, f0_
    # Ts = Ts_

    s = sp.symbols('s')
    z = sp.symbols('z')
    w_0N, zeta_0N, w_0D, zeta_0D = sp.symbols('w_0N zeta_N w_0D zeta_D')
    f_0N, f_0D = sp.symbols('f_0N f_0D')

    equ = [sp.Eq(d_depth, (f_0D/f_0N)**2),
           sp.Eq(f_0D, fc),
           sp.Eq(w_0N, 2*sp.pi*f_0N),
           sp.Eq(w_0D, 2*sp.pi*f_0D)]
    sol = sp.solve(equ, w_0N, w_0D, f_0N, f_0D)
    w_0N = sol[0][0]
    w_0D = sol[0][1]
    f_0N = sol[0][2]
    f_0D = sol[0][3]

    # %% [markdown]
    #
    sys = d_depth*(s**2 + 2*zeta_0N*w_0N*s + w_0N**2) / \
        (s**2 + 2*zeta_0D*w_0D*s + w_0D**2)
    # %% [markdown]
    #
    sys = sys.subs(s, 2/Ts*(z-1)/(z+1))
    sys_tmp = sys

    sys_tmp = sp.simplify(sys_tmp)
    sys_tmp = sp.expand(sys_tmp, frac=True)
    sys_tmp = sp.collect(sys_tmp, z)
    sys_tmp_value = sys_tmp
    sys_tmp_value = sys_tmp.subs(
        {Ts: Ts_, d_depth: d_depth_, fc: fc_, zeta_0N: zeta_0N_, zeta_0D: zeta_0D_})
    # sys_tmp_value = sys_tmp.subs({Ts:1e-4, d_depth:0.5, f_width:3, f0:15})

    m, n = ztrans2diffexpr(sys_tmp_value)
